#include "PID.h"

void PID_Init(PIDControl *pid)
{
    pid->Kp = 0;
    pid->Ki = 0;
    pid->Kd = 0;

    pid->target = 0;
    pid->actrul = 0;

    pid->lastError = 0;
    pid->prevError = 0;

    pid->sumError = 0;
}

double PID_LocCal(PIDControl *pid, uint8_t Ctrl)
{
    double iError = 0;
    double dError = 0;
    iError = pid->target - pid->actrul;
    pid->sumError += iError;
    dError = iError - pid->lastError;
    pid->lastError = iError;

    return ((Ctrl & 0x01 ? pid->Kp * iError : 0) + (Ctrl & 0x02 ? pid->Ki * pid->sumError : 0) + (Ctrl & 0x04 ? pid->Kd * dError : 0));
}

double PID_IncCal(PIDControl *pid, uint8_t Ctrl)
{
    double iError = 0;
    double iIncpid = 0;
    iError = pid->target - pid->actrul;

    iIncpid = ((Ctrl & 0x01 ? pid->Kp * iError : 0) - (Ctrl & 0x02 ? pid->Ki * pid->lastError : 0) + (Ctrl & 0x04 ? pid->Kd * pid->prevError : 0));
    pid->prevError = pid->lastError;
    pid->lastError = iError;

    return iIncpid;
}
